#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <tf/transform_listener.h>
#include "common_msg/UpperInfo.h"
#include "common_msg/SonarInfo.h"
#include "common_msg/Carto2DriverInfo.h"

#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
//#include "map_receiver.h"
#include "tcp_server.h"
#include "shm.h"
#include "caltime.h"

#include "task_manager.h"
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#include <sstream>
#include <fstream>
#include <string>
#include <functional>

using namespace boost::property_tree;


int gl_controlMode = AUTO_MODE;
int gl_moveFlag = 1;

vector<string> gl_code_list;
int gl_patrol_num = 2;
int gl_sentry_num = 4;

vector <unsigned char> robotMap;
vector <unsigned char> expandMap;

extern struct InterFaceInfo soldierOneInfo;
extern const char* stateCode[14];

TaskManager task_manager;
void SignalHandle(const char * data,int size)
{
	/*release source*/
	exit(0);  
}

void sighandle(int sig)
{
	/*release source*/
	exit(0);  
}
void sonarCallback(const common_msg::SonarInfo::ConstPtr& sonic_msg)
{

}
void cartoCallback(const common_msg::Carto2DriverInfo::ConstPtr& carto_msg)
{
	int move_flag = carto_msg->moveFlag;
	if( move_flag != 0)
	{
		task_manager.setPosState(true);
	}
	else
	{
		task_manager.setPosState(false);
	}
	
}
void getRobotPose()
{
	ros::Rate r(10);
	while(ros::ok())
	{
		tf::Stamped<tf::Pose> global_pose;
		string global_frame_ = "map";
		string robot_base_frame_ = "base_footprint";
		global_pose.setIdentity();
		tf::Stamped < tf::Pose > robot_pose;
		robot_pose.setIdentity();
		robot_pose.frame_id_ = robot_base_frame_;
		robot_pose.stamp_ = ros::Time();
		ros::Time current_time = ros::Time::now();  // save time for checking tf delay later
		tf::TransformListener tf_;
		geometry_msgs::Pose pose;
		try
		{
			tf_.waitForTransform(global_frame_,robot_base_frame_,ros::Time(0),ros::Duration(0.5));
			tf_.transformPose(global_frame_, robot_pose, global_pose);			
			poseTFToMsg(global_pose,pose);
			task_manager.setRobotPose(pose);
		}
		catch(...)
		{
			ROS_WARN("can not get robot pose");
		}
		r.sleep();
	}
}
std::vector<std::string> split(std::string str,std::string pattern)
{
	std::string::size_type pos;
	std::vector<std::string> result;
	str+=pattern;
	int size=str.size();

	for(int i=0; i<size; i++)
	{
		pos=str.find(pattern,i);
		if(pos<size)
		{
			std::string s=str.substr(i,pos-i);
			result.push_back(s);
			i=pos+pattern.size()-1;
		}
	}
	return result;
}
bool get_task(char fileUri[])
{
	ifstream in(fileUri);
	vector <Task> task_list;
	ptree task_info,task_array,task;
	try{
		read_json(in, task_info);
	}
	catch(...)
	{
		return false;
	}
	in.close();
	task_array = task_info.get_child("task_list");
	Task task_temp;
	for(ptree::iterator it=task_array.begin();it!=task_array.end();it++)
	{
		task = it->second;
		task_temp.task_name = task.get<string>("task_name");
		task_temp.route_name = task.get<string>("route_name");
		task_temp.loop = task.get<bool>("loop");
		task_temp.loop_count = task.get<int>("loop_count");
		task_temp.start_time = task.get<string>("start_time");
		task_temp.end_time = task.get<string>("end_time");
		task_list.emplace_back(task_temp);
	}
	if(task_list.size()>0)
	{
		task_manager.setTaskList(task_list);
		return true;
	}
	else 
	{
		return false;
	}

}
bool get_goal(char fileUri[])
{
	ifstream in(fileUri);	
	vector<string> route_linesName;
	ptree route_info,route_lines;
	try{
		read_json(in, route_info);
	}
	catch(...)
	{
		return false;
	}
	in.close();

	route_lines = route_info.get_child("route_lines");
	string line_name;
	ptree temp1,temp2,temp3,temp4,temp5;
	geometry_msgs::Pose pose;
	vector<vector<geometry_msgs::Pose> > route_lines_pose;
	for(ptree::iterator it=route_lines.begin();it!=route_lines.end();it++)
	{
		temp1 = it->second;
		line_name = temp1.get<string>("name");
		route_linesName.push_back(line_name);

		temp2 = temp1.get_child("route_line");
		vector<geometry_msgs::Pose> route_line_pose;
		for(ptree::iterator it=temp2.begin();it!=temp2.end();it++)
		{
			temp3 = it->second;
			temp4 = temp3.get_child("position");
			temp5 = temp3.get_child("orientation");

			pose.position.x = temp4.get<double>("x");
            pose.position.y = temp4.get<double>("y");
            pose.position.z = temp4.get<double>("z");
            pose.orientation.w = temp5.get<double>("w");
			pose.orientation.x = temp5.get<double>("x");
			pose.orientation.y = temp5.get<double>("y");
			pose.orientation.z = temp5.get<double>("z");
			route_line_pose.push_back(pose);
		}
		route_lines_pose.push_back(route_line_pose);
	}
	
	if(route_lines_pose.empty() || !task_manager.setRouteLines(route_linesName,route_lines_pose))
	{
		return false;
	}
	return true;
}

void updateState()
{

}
void navigation_thread(actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> *ac,bool *goalSend)
{
	while(ros::ok())
	{
		usleep(100000); 
		if(AUTO_MODE == gl_controlMode) 
		{
			move_base_msgs::MoveBaseGoal goal;
			taskState state = task_manager.getGoal(goal);	
			if(state != TASK_OK)
			{
				//ROS_WARN_STREAM_THROTTLE(1.0,"no more point in task, ignoring");
				continue;
			}
			ac->sendGoal(goal);
			*goalSend = true;
			bool finished_within_time = ac->waitForResult(ros::Duration(600));		
			if(!finished_within_time)
			{
				ac->cancelGoal();
				ROS_INFO("Timed out achieving goal");
			}
			else
			{
				if(ac->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
				{
					ROS_INFO("Goal succeeded!");
				}
				else
				{
					ROS_INFO("The base failed for some reason");
				}
			}						
		}
		usleep(2000);
	}
	exit(0);
}
int main(int argc,char *argv[])
{
	ros::init(argc,argv,"zubot_nav");
		
	ros::NodeHandle _Node;
	ros::Subscriber utralsonic = _Node.subscribe("/utralsonic",2, sonarCallback);
	ros::Subscriber carto2driver = _Node.subscribe("/carto_position",2, cartoCallback);
	ros::Publisher upperInfoPub = _Node.advertise<common_msg::UpperInfo>("/upper_info",2);				//Upper

	ros::Publisher cmdPub = _Node.advertise<geometry_msgs::Twist>("/cmd_vel",2);


	shm_init();
	//socket thread
	std::thread paththreadSocket(socket_info);
	paththreadSocket.detach();	

	signal(SIGINT,sighandle); //Ctrl+C 
	geometry_msgs::Twist cmd_msg;
	cmd_msg.linear.x = 0.0;
	cmd_msg.linear.y = 0.0;
	cmd_msg.linear.z = 0.0;
	cmd_msg.angular.x = 0.0;
	cmd_msg.angular.y = 0.0;
//	while(ros::ok()&& !task_manager.isPosValid()) 		//
	{		
		cmd_msg.angular.z = -0.6;
		cmdPub.publish(cmd_msg);
		usleep(10000);									//10ms
		ros::spinOnce();
	}

	sleep(1.0);

	Caltime cal_time;
	cal_time.set_start_time();
	double time_ = 0.;
//	while(ros::ok()&&time_<12000)
	{
		usleep(50000);
		cal_time.set_stop_time();
		time_ = cal_time.cal_time_ms();
		cmd_msg.angular.z = 0.4;
		cmdPub.publish(cmd_msg);
	}
	
	char taskUrl[] = {"/home/smartgo/package_ws/src/zubot_nav/config/routeline/task.json"};
	if(!get_task(taskUrl))
	{
		ROS_ERROR("cannot get task file!!!");
		return 0;
	}
	char goalUrl[] = {"/home/smartgo/package_ws/src/zubot_nav/config/routeline/route_line.json"};
	if(!get_goal(goalUrl))
	{
		ROS_ERROR("cannot get goal file!!!");
		return 0;
	}

	std::thread getPose(getRobotPose);
	getPose.detach();

    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base",true);
	ac.waitForServer();	

	std::function<void()> task_schedule = std::bind(&TaskManager::schedule,&task_manager);
	std::thread task_thread(task_schedule);
	task_thread.detach();

	bool goalSend = false;
	std::thread navthread(navigation_thread,&ac,&goalSend);
	navthread.detach();


	ros::Rate r(20);
	while(ros::ok())
    {
		gl_controlMode = shm_read();	
		common_msg::UpperInfo upperInfo;
		upperInfo.headControl = soldierOneInfo.headControl;
		upperInfoPub.publish(upperInfo);
		if(MANUAL_MODE == gl_controlMode)
		{
            if(goalSend && (ac.getState() == actionlib::SimpleClientGoalState::ACTIVE))//active
			{
				goalSend = false;
				ac.cancelGoal();
			}			    
		}
		ros::spinOnce();
		r.sleep();
	}//while(ros::ok())
    if(goalSend)
	{
		goalSend = false;
		ac.cancelGoal();
	}
	//google::ShutdownGoogleLogging();
	return 0;
}
